/**
*  \file analog_controller.c
*
*  \brief
*
*  \author Michael J. Hohmann  <michael.hohmann@linde-mh.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "analog_controller.h"
#include "../assertor/assertor.h"
#include "../pal/pal_mem.h"
#include "../mlib/mlib.h"
#include "../cancom/cancom.h"

/*===============================================[ private defines   ]================================================*/

/*===============================================[ private datatypes ]================================================*/

//static const S16 analog_controller_percent_curve[]= { -10000, -10000, 0, 10000, 10000 };



/*===============================================[ private variables ]================================================*/

/*===============================================[ private functions ]================================================*/

/*================================================[ inline functions ]================================================*/

/*================================================[ public functions ]================================================*/

RC analog_controller_construct( struct analog_controller_obj *me, struct interpol_u16_s16_obj *ptable , U16 error_tolerance, U16 zero_tolerance )
{	
   ASSERTOR_PRE( NIL != me,"NULL is not allowed as object pointer." );
   pal_memzero( me , sizeof *me );

   me->ptable=ptable;

   me->error_tolerance=error_tolerance;
   me->zero_tolerance=zero_tolerance;

   return RC_SUCCESS;
}


void analog_controller_process( struct analog_controller_obj *me, F32 value1 , F32 value2, F32 reference )
{
	ASSERTOR_PRE( value1>=0,"Positive value expected");
	ASSERTOR_PRE( value2>=0,"Positive value expected");
	ASSERTOR_PRE( reference>=0,"Positive value expected");
	{
		U16 uvalue1=   (U16)(value1 / can_resol_voltage_sensor);
		U16 uvalue2=   (U16)(value2 / can_resol_voltage_sensor);
		U16 ureference=(U16)(reference / can_resol_voltage_sensor);

		U16 input1_percent = 0;
		U16 input2_percent = 0;

		if( ureference!= 0)
		{
			input1_percent = muldiv_u16( uvalue1, 10000, ureference);
			input2_percent = muldiv_u16( uvalue2, 10000, ureference);
		}
		/* always check for error condition */
		{
			U16 sum=input1_percent+input2_percent;

			if(( sum > 10000+me->error_tolerance) ||  ( sum < 10000 - me->error_tolerance ))
			{
				me->state =  analog_controller_state_error;
			}

			/*if(( value1 > 480) ||  ( value1 < 20 ))
			{
				me->state =  analog_controller_state_error;
			}

			if(( value2 > 480) ||  ( value2 < 20 ))
			{
				me->state =  analog_controller_state_error;
			}*/
		}

		switch( me->state)
		{
			case analog_controller_state_error:
			{
				me->current_value=0;
				break;
			}
			case analog_controller_state_operational:
			{
				S16 value=interpol_u16_s16(me->ptable,input1_percent);
				if( value > 10000 )
				{
					// enter error here
					me->current_value=0;
				}
				else
				{
					me->current_value=value;
				}
				break;
			}
			case analog_controller_state_wait_for_neutral:
			{
				S16 position = interpol_u16_s16(me->ptable,input1_percent);
				me->current_value=0;

				if(( position < me->zero_tolerance ) && ( position > -me->zero_tolerance ) )
				{
					if( 1 ) /* todo time delay */
					{
						me->state=analog_controller_state_operational;
					}
				}
				break;
			}
			default:
			{
				ASSERTOR_ILLEGAL_DEFAULT("Invalid state");
				break;
			}

		}
	}
	ASSERTOR_POST( me->current_value <= 10000," value has to be smaller or equal than 100% ");
	ASSERTOR_POST( me->current_value >=-10000," value has to be larger than 100% ");


}

enum analog_controller_state analog_controller_get( struct analog_controller_obj const * me, S16 *p_neutral, S16 *p_value )
{
	*p_value=me->current_value;
	return me->state;
}

/*===============================================[ private functions ]================================================*/


/***********************************************************************************************************************
*                             Copyright 2019 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/
